#include "v4l2_camera.h"


void V4L2Camera::init(const char* device, uint width, uint height) {
    // 属性初始化
    this->width = width;
    this->height = height;
    this->capturing = false;
    this->getting_frame = false;
    strcpy(this->device, device);
    this->img = new ImgBuf;
    img->length = this->width * this->height * 3;
    this->img->start = (unsigned char*)malloc(this->img->length);
    img->width = this->width;
    img->height = this->height;
    
    // 1.打开UVC设备
    this->fd = v4l2_init(this->device);
    if (this->fd < 0) {
        printf("ERROR: Can not open device: %s\n\n", device);
        exit(-1);
    }

    // 2.获取UVC支持格式
    Status rsp = v4l2_get_support_formats(this->fd);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }

    // 3.配置UVC采集格式
    rsp = v4l2_set_capture_format(this->fd, this->width, this->height);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }

    // 4.申请内核缓冲区
    rsp = v4l2_request_core_buffer(this->fd);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }

    // 5.把内核缓冲区映射到用户空间
    this->buffer = (ImgBuf*)malloc(4 * sizeof(ImgBuf));
    rsp = v4l2_map_buffer(this->fd, this->buffer);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
}

V4L2Camera::V4L2Camera(const char* device) {
    this->init(device, 320, 180);
}

V4L2Camera::V4L2Camera(const char* device, uint width, uint height) {
    this->init(device, width, height);
}

void V4L2Camera::start() {
    // 6.开始采集
    if (this->capturing) return; //已经开始采集了
    this->capturing = true;
    Status rsp = v4l2_start_stream(this->fd);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
}

ImgBuf* V4L2Camera::get_jpg_frame() {
    // 7.采集数据 - 上锁
    if (!this->capturing) this->start(); // 先开始采集
    if (this->getting_frame) return &(this->buffer[this->readbuffer.index]); // 已经上锁了
    this->getting_frame = true;
    Status rsp = v4l2_get_frame(this->fd, this->readbuffer);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
    this->buffer[this->readbuffer.index].length = this->readbuffer.length;
    this->buffer[this->readbuffer.index].width = this->width;
    this->buffer[this->readbuffer.index].height = this->height;
    return &(this->buffer[this->readbuffer.index]);
}

void V4L2Camera::release_frame() {
    // 8.采集数据 - 解锁
    if (!this->getting_frame) return;   // 已经解锁了
    this->getting_frame = false;
    Status rsp = v4l2_release_frame(this->fd, this->readbuffer);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
}

void V4L2Camera::stop() {
    // 9.停止采集
    if (this->getting_frame) this->release_frame(); // 先解锁
    if (!this->capturing) return; // 已经停止了
    this->capturing = false;
    Status rsp = v4l2_stop_stream(this->fd);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
}

V4L2Camera::~V4L2Camera() {
    // 8.9.解锁并停止采集
    this->stop();
    // 10.释放映射
    Status rsp = v4l2_munmap_buffer(this->buffer);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
    // 11.关闭设备
    rsp = v4l2_close(this->fd);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
    // 释放所用内存空间
    free(img->start);
    delete img;
    free(this->buffer);
}

void V4L2Camera::info() {
    Status rsp = v4l2_get_support_formats(this->fd);
    if (rsp != SUCCESS) {
        printf("Error! code:%d.\n\n", rsp);
        exit(rsp);
    }
}

// void V4L2Camera::test_time() {
//     struct timeval ts, te;
    
//     this->start();
//     this->get_jpg_frame();
//     gettimeofday(&ts, NULL);
//     for (int i = 0; i < 100; ++i) {
//         Jpg2Rgb(this->buffer[this->readbuffer.index], this->img);
//     }
//     gettimeofday(&te, NULL);
//     this->release_frame();

//     long long start_time = ts.tv_sec * 1000000 + ts.tv_usec;
//     long long end_time = te.tv_sec * 1000000 + te.tv_usec;
//     long long delta_time = end_time - start_time;
//     double fps = 100000000.0 / delta_time;
//     printf("\nAverage Decompress Time %lf ms, %lf FPS.\n", (double)delta_time / 100000, fps);

//     gettimeofday(&ts, NULL);
//     for (int i = 0; i < 100; ++i) {
//         this->get_frame();
//     }
//     gettimeofday(&te, NULL);
//     this->stop();
//     start_time = ts.tv_sec * 1000000 + ts.tv_usec;
//     end_time = te.tv_sec * 1000000 + te.tv_usec;
//     delta_time = end_time - start_time;
//     fps = 100000000.0 / delta_time;
//     printf("Average Capture Time %lf ms, %lf FPS.\n\n", (double)delta_time / 100000, fps);
// }